#!/usr/bin/env python

import math
import numpy as np
import matplotlib.pyplot as plt
import time
import ros
import rospy
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped

NOISE_NUM = 0.1                 # 道路中心线增加扰动
LENGTH_TO_BOUNDARY = 1.5        # 中心线到道路左右边界距离
STEP_FACTOR = 1                 # 两个点之间的距离

def rotationTra(tra, point, angle):
    """ 输入一条轨迹，返回按 point 旋转 angle 角度后的轨迹 """
    newTra = np.zeros_like(tra)
    x0, y0 = point[0], point[1]
    # 逆时针
    newTra[:, 0] = (tra[:, 0]-x0)*np.cos(angle) - (tra[:, 1]-y0)*np.sin(angle)
    newTra[:, 1] = (tra[:, 0]-x0)*np.sin(angle) + (tra[:, 1]-y0)*np.cos(angle)
    # 顺时针
    # newTra[:, 0] = (tra[:, 0]-x0)*np.cos(angle) + (tra[:, 1]-y0)*np.sin(angle)
    # newTra[:, 1] = (tra[:, 1]-y0)*np.cos(angle) - (tra[:, 0]-x0)*np.sin(angle)
    return newTra


def cal_heading(point_1, point_2):
    """ 计算点 point_1 的heading角度 """
    angle = 0
    delta_x = point_2[0] - point_1[0]
    delta_y = point_2[1] - point_1[1]
    if delta_x == 0.0:
        angle = np.pi / 2.0
    else:
        angle = math.atan(math.fabs(delta_y / delta_x))
    if (delta_x < 0.0) and (delta_y >= 0):
        angle = np.pi - angle
    elif (delta_x < 0.0) and (delta_y < 0):
        angle = np.pi + angle
    elif (delta_x >= 0.0) and (delta_y < 0.0):
        angle = np.pi * 2.0 - angle
    return angle


def gen_sl_path(path):
    """
    计算路径的 heading 和 kappa，生成全局路径参考线
    """
    heading, kappa = [], []
    for index in np.arange(0, path.shape[0] - 1, 1):
        theta = cal_heading(path[index, :], path[index+1, :])
        dis = np.linalg.norm(path[index, :] - path[index+1, :])
        heading.append(theta)
        kappa.append(theta / dis)
    # 将最后一个点的heading 和 kappa 设置为 0，
    heading.append(0.0)
    kappa.append(0.0)
    heading = np.array(heading).reshape([-1, 1])
    kappa = np.array(kappa).reshape([-1, 1])
    
    path = np.hstack([path, heading, kappa])
    return path


def gen_lane_info(path):
    """
    计算路径偏航角的cos和sin值，以及与左右道路边缘的距离
    """
    cos_list, sin_list = [], []
    l_length, r_length = [], []
    for index in np.arange(0, path.shape[0] - 1, 1):
        theta = cal_heading(path[index, :], path[index+1, :])
        cos_list.append(math.cos(theta))
        sin_list.append(math.sin(theta))
        l_length.append(LENGTH_TO_BOUNDARY)
        r_length.append(LENGTH_TO_BOUNDARY)
    
    # path = np.delete(path, path.shape[])
    cos_list = np.array(cos_list).reshape([-1, 1])
    sin_list = np.array(sin_list).reshape([-1, 1])
    l_length = np.array(l_length).reshape([-1, 1])
    r_length = np.array(r_length).reshape([-1, 1])
    
    path = np.hstack([path[0:-1, :], cos_list, sin_list, l_length, r_length])
    return path


def calcuBoundary(laneInfo):
    """
    输入一路段信息，计算边界轨迹。
    返回(中心线、左边界，右边界)数据 shape:(N, 6)
    """
    xpoint = laneInfo[:,0]
    ypoint = laneInfo[:,1]
    cos = laneInfo[:, 2]
    sin = laneInfo[:, 3]
    lLength = laneInfo[:, 4]
    rLength = laneInfo[:, 5]
    # left boundary
    l_b_x = xpoint - lLength*sin
    l_b_y = ypoint + lLength*cos
    # right boundary
    r_b_x = xpoint + rLength*sin
    r_b_y = ypoint - rLength*cos
    # laneInfo shape: (dataLength, 6) (中心线、左边界，右边界)
    return np.vstack([xpoint, ypoint, l_b_x, l_b_y, r_b_x, r_b_y]).T


def gen_path_1():
    """生成全局道路坐标
    """
    road_c_x, road_c_y, ryaw, rc = [], [], [], []
    step_curve = 0.01 * math.pi
    step_line = 1

    # 直线段
    for ix in np.arange(0, 50, step_line):
            road_c_x.append(ix)
            road_c_y.append(0)
    # 对道路添加扰动
    for i in np.arange(5, 50, 10):
        road_c_y[i] += 0.5
        road_c_y[i+1] += 0.5

    # 半圆
    cx, cy, cr = 50, 15, 15
    theta = np.arange(-math.pi / 2.0, math.pi / 2.0, 1.0 / 15)
    for itheta in theta:
        road_c_x.append(cx + cr * math.cos(itheta))
        road_c_y.append(cy + cr * math.sin(itheta))
    
    for ix in np.arange(50, 0, -step_line):
        road_c_x.append(ix)
        road_c_y.append(30)

    road_c_x = np.array(road_c_x).reshape([-1, 1])
    road_c_y = np.array(road_c_y).reshape([-1, 1])
    path = np.hstack([road_c_x, road_c_y])

    # print(path.shape)
    
    ### 对路径进行旋转（逆时针方向）
    angle = np.pi / 6.0
    path = rotationTra(path, [0, 0], angle=angle)

    global_path = gen_sl_path(path)

    # np.save('./src/my_planning/controllers/path/global_path_1.npy', global_path[:-1, :])
    # plt.plot(global_path[:, 0], global_path[:, 1], marker='.', color='r')
    # plt.show()
    # print(global_path.shape[0])
    return global_path


def getStepRad(r):
    return STEP_FACTOR * 1 / r


def gen_path_2():
    """生成环状道路
    """
    need_add_noise = rospy.get_param("need_add_noise")
    
    rx, ry, ryaw, rc = [], [], [], []
    step_curve = 0.01 * math.pi
    step_line = 1 * STEP_FACTOR

    # 直线段
    for ix in np.arange(0, 60, step_line):
            rx.append(ix)
            ry.append(0)

    # 半圆
    cx, cy, cr = 60, 20, 20
    theta = np.arange(-math.pi / 2.0, math.pi / 2.0, getStepRad(cr))
    for itheta in theta:
        rx.append(cx + cr * math.cos(itheta))
        ry.append(cy + cr * math.sin(itheta))
    
    cx, cy, cr = 60, 0, 40
    theta = np.arange(math.pi / 2.0, math.pi, getStepRad(cr))
    for itheta in theta:
        rx.append(cx + cr * math.cos(itheta))
        ry.append(cy + cr * math.sin(itheta))
    
    for iy in np.arange(0, -20, -step_line):
        rx.append(20)
        ry.append(iy)

    cx, cy, cr = 0, -20, 20
    theta = np.arange(2 * math.pi, math.pi / 2.0, -getStepRad(cr))
    for itheta in theta:
        rx.append(cx + cr * math.cos(itheta))
        ry.append(cy + cr * math.sin(itheta))

    rx = np.array(rx).reshape([-1, 1])
    ry = np.array(ry).reshape([-1, 1])
    path = np.hstack([rx, ry])
    
    ### 对路径进行旋转（逆时针方向）
    angle = np.pi / 6.0
    path = rotationTra(path, [0, 0], angle=angle)

    laneInfo = gen_lane_info(path)
    roadPoint = calcuBoundary(laneInfo=laneInfo)

    if need_add_noise:
        # 对道路中心线添加干扰
        noise = np.random.normal(0, NOISE_NUM, (roadPoint.shape[0], 2))
        noise = np.hstack([noise, np.zeros((roadPoint.shape[0], 4))])
        roadPoint += noise

    # np.save('./src/my_planning/controllers/path/global_path_1.npy', global_path[:-1, :])
    return roadPoint


def gen_path_3():
    """生成圆环
    """
    need_add_noise = rospy.get_param("need_add_noise")
    
    rx, ry, ryaw, rc = [], [], [], []
    step_curve = 0.01 * math.pi
    step_line = 1 * STEP_FACTOR

    # 直线段
    for ix in np.arange(0, 60, step_line):
            rx.append(ix)
            ry.append(0)

    # 半圆
    cx, cy, cr = 30, 52, 60
    theta = np.arange(-math.pi / 3.0, math.pi * 4 / 3.0, getStepRad(cr))
    for itheta in theta:
        rx.append(cx + cr * math.cos(itheta))
        ry.append(cy + cr * math.sin(itheta))
    
    rx = np.array(rx).reshape([-1, 1])
    ry = np.array(ry).reshape([-1, 1])
    path = np.hstack([rx, ry])

    ### 对路径进行旋转（逆时针方向）
    angle = np.pi / 6.0
    path = rotationTra(path, [0, 0], angle=angle)

    laneInfo = gen_lane_info(path)
    roadPoint = calcuBoundary(laneInfo=laneInfo)

    if need_add_noise:
        # 对道路中心线添加干扰
        noise = np.random.normal(0, NOISE_NUM, (roadPoint.shape[0], 2))
        noise = np.hstack([noise, np.zeros((roadPoint.shape[0], 4))])
        roadPoint += noise

    return roadPoint


def gen_path_4():
    """生成大圆环
    """
    need_add_noise = rospy.get_param("need_add_noise")
    
    rx, ry, ryaw, rc = [], [], [], []

    # 半圆
    cx, cy, cr = 0, 60, 60
    theta = np.arange(-math.pi / 2.0, math.pi * 3 / 2.0, getStepRad(cr))
    for itheta in theta:
        rx.append(cx + cr * math.cos(itheta))
        ry.append(cy + cr * math.sin(itheta))
    
    rx = np.array(rx).reshape([-1, 1])
    ry = np.array(ry).reshape([-1, 1])
    path = np.hstack([rx, ry])

    laneInfo = gen_lane_info(path)
    roadPoint = calcuBoundary(laneInfo=laneInfo)

    if need_add_noise:
        # 对道路中心线添加干扰
        noise = np.random.normal(0, NOISE_NUM, (roadPoint.shape[0], 2))
        noise = np.hstack([noise, np.zeros((roadPoint.shape[0], 4))])
        roadPoint += noise

    return roadPoint


def pub_global_path(path_sl):
    global_path_pub  = rospy.Publisher('global_path_center', Path, queue_size=10)
    global_path_l_pub = rospy.Publisher('global_path_left', Path, queue_size=10)
    global_path_r_pub = rospy.Publisher('global_path_right', Path, queue_size=10)

    rospy.init_node('Gen_Global_Path', anonymous=True)
    rate = rospy.Rate(10)
    global_path = Path()
    global_path_l = Path()
    global_path_r = Path()

    global_path.header.frame_id = global_path_l.header.frame_id = global_path_r.header.frame_id = 'world'
    global_path.header.stamp = global_path_l.header.stamp = global_path_r.header.stamp = rospy.Time.now()
    
    while not rospy.is_shutdown():
        global_path.poses.clear()
        global_path_l.poses.clear()
        global_path_r.poses.clear()
        
        for index in np.arange(0, path_sl.shape[0], 1):
            
            pose = PoseStamped()
            pose_l = PoseStamped()
            pose_r = PoseStamped()

            pose.header.frame_id = pose_l.header.frame_id = pose_r.header.frame_id = 'world'
            pose.header.stamp = pose_l.header.stamp = pose_r.header.stamp = rospy.Time.now()
            # 中心线坐标
            pose.pose.position.x = path_sl[index, 0]
            pose.pose.position.y = path_sl[index, 1]
            # 左边界坐标
            pose_l.pose.position.x = path_sl[index, 2]
            pose_l.pose.position.y = path_sl[index, 3]
            # 右边界坐标
            pose_r.pose.position.x = path_sl[index, 4]
            pose_r.pose.position.y = path_sl[index, 5]
            pose.pose.position.z = pose_l.pose.position.z = pose_r.pose.position.z = 0

            pose.pose.orientation.x = pose_l.pose.orientation.x = pose_l.pose.orientation.x = 0.0
            pose.pose.orientation.y = pose_l.pose.orientation.y = pose_l.pose.orientation.y = 0.0
            pose.pose.orientation.z = pose_l.pose.orientation.z = pose_l.pose.orientation.z = 0.0
            pose.pose.orientation.w = pose_l.pose.orientation.w = pose_l.pose.orientation.w = 0.0
            global_path.poses.append(pose)
            global_path_l.poses.append(pose_l)
            global_path_r.poses.append(pose_r)

        global_path_pub.publish(global_path)
        global_path_l_pub.publish(global_path_l)
        global_path_r_pub.publish(global_path_r)
        rate.sleep()


if __name__ == '__main__':
    path_sl = gen_path_4()
    # print(path_sl.shape)
    # plt.plot(path_sl[:, 0], path_sl[:, 1], marker='.', color='r')
    # plt.plot(path_sl[:, 2], path_sl[:, 3], color='y')
    # plt.plot(path_sl[:, 4], path_sl[:, 5], color='y')
    # plt.show()
    pub_global_path(path_sl=path_sl)

    # print(math.asin())
